import serial
import time
import struct


class FOCMotorController:
    def __init__(self, port):
        self.serial_object = serial.Serial(port,115200)
        self.serial_object.read_all()

    def enable(self):
        packet = "QDrive enable\n"
        self.serial_object.write(packet.encode('utf-8'))
        self.serial_object.read_all()
        time.sleep(0.02)

    def disable(self): 
        packet = "QDrive disable\n"
        self.serial_object.write(packet.encode('utf-8'))
        self.serial_object.read_all()
        time.sleep(0.02)
    
    def setAngle(self, angle):
        packet = ""
        packet += "QDrive ctrl angle "
        packet += '%0.5f'%angle + "\n"
        self.serial_object.write(packet.encode('utf-8'))
        self.serial_object.read_all()
        time.sleep(0.02)

    def setSpeed(self, speed, lowSpeedMode = False):
        packet = ""
        if lowSpeedMode:
            packet += "QDrive ctrl low_speed "
        else:
            packet += "QDrive ctrl speed "
        packet += '%0.5f'%speed + "\n"
        self.serial_object.write(packet.encode('utf-8'))
        self.serial_object.read_all()
        time.sleep(0.02)
